import math


class MFACclass(object):

    def __init__(self, model):
        self.model = model

        self.MFAC_out = 0  # 初始化t时刻控制器输出，推力或舵角（未乘扭矩系数）
        self.pre_MFAC_out = 0  # 初始化t-1时刻控制器输出，推力或舵角（未乘扭矩系数）
        self.pre_pre_MFAC_out = 0  # 初始化t-2时刻控制器输出，推力或舵角（未乘扭矩系数）
        self.b = 0  # # t-1与t-2时刻控制器输出变化

        self.pre_cur_val = 0  # t-1时刻实际值（速度或航向） ， cur_val为t时刻当前实际值（速度或航向）
        self.error = 0  # 初始化t时刻航向/航速误差值
        self.a = 0  # t到t-1时刻航向或航速变化量

        self.rou = None  # MFAC控制系数
        self.namuta = None
        self.yita = None
        self.miu = None
        self.q = None
        self.kr = None
        self.fai = None

    def set_value(self):
        if self.model == 'A':  # 若为<航向>控制

            # self.rou = 1  # 差分型MFAC<航向>控制参数
            # self.namuta = 0.01
            # self.yita = 0.01
            # self.miu = 10
            # self.q = 0.001
            # self.kr = 3.5
            # self.fai = 0.01  # <航向>控制器中伪偏导数初始值
            # print(f"MFAC:{self.rou, self.namuta, self.yita, self.miu, self.q, self.kr, self.fai}")
            self.rou = 1  # 差分型MFAC<航向>控制参数
            self.namuta = 0.0001
            self.yita = 0.01
            self.miu = 10
            self.q = 0.001
            self.kr = 3
            self.fai = 0.01  # <航向>控制器中伪偏导数初始值
            print(f"MFAC:{self.rou, self.namuta, self.yita, self.miu, self.q, self.kr, self.fai}")

        elif self.model == 'V':  # 若为<航速>控制

            # self.rou = 1  # 经典MFAC<航速>控制参数
            # self.namuta = 8.3
            # self.yita = 0.001
            # self.miu = 15
            # self.q = 0.001
            # self.fai = 0.1  # <航速>控制器中伪偏导数初始值
            # print(f"MFAC:{self.rou, self.namuta, self.yita, self.miu, self.fai}")
            self.rou = 1  # 经典MFAC<航速>控制参数
            self.namuta = 0.005
            self.yita = 0.3
            self.miu = 10
            self.q = 0.001
            self.fai = 0.1  # <航速>控制器中伪偏导数初始值
            print(f"MFAC:{self.rou, self.namuta, self.yita, self.miu, self.fai}")

    def calculate(self, target, cur_val):
        if self.model == 'A':  # 航向控制器

            self.error = target - cur_val  # 误差
            self.a = cur_val - self.pre_cur_val  # t到t-1时刻航向或航速变化量
            self.b = self.pre_MFAC_out - self.pre_pre_MFAC_out  # t-1与t-2时刻控制器输出变化
            self.fai = self.fai + (self.yita * self.b * (self.a - self.fai * self.b)) / (self.miu + self.b ** 2)
            if (abs(self.fai) <= self.q) or (abs(self.b) <= self.q) or self.fai <= 0:
                self.fai = 0.01

            self.pre_pre_MFAC_out = self.pre_MFAC_out  # 更新迭代变量
            self.pre_MFAC_out = self.MFAC_out
            self.pre_cur_val = cur_val
            self.MFAC_out = self.MFAC_out + self.rou * self.fai * (self.error - self.kr * (self.a / 0.1)) / (
                        self.namuta + self.fai ** 2)

            if self.MFAC_out >= 30:
                self.MFAC_out = 30
            elif self.MFAC_out <= -30:
                self.MFAC_out = -30

            self.MFAC_out = self.MFAC_out * 81.7

            return self.MFAC_out

        elif self.model == 'V':  # 航速控制器

            self.error = target - cur_val  # 误差
            self.a = cur_val - self.pre_cur_val  # t到t-1时刻航向或航速变化量
            self.b = self.pre_MFAC_out - self.pre_pre_MFAC_out  # t-1与t-2时刻控制器输出变化
            self.fai = self.fai + (self.yita * self.b * (self.a - self.fai * self.b)) / (self.miu + self.b ** 2)
            # self.sign_fai = math.copysign(1, self.fai)
            if (abs(self.fai) <= self.q) or (abs(self.b) <= self.q) or self.fai <= 0:
                self.fai = 0.1

            self.pre_pre_MFAC_out = self.pre_MFAC_out  # 更新迭代变量
            self.pre_MFAC_out = self.MFAC_out
            self.pre_cur_val = cur_val
            self.MFAC_out = self.MFAC_out + self.rou * self.fai * self.error / (self.namuta + self.fai ** 2)

            if self.MFAC_out > 680:
                self.MFAC_out = 680

            return self.MFAC_out
